#include <stdio.h>
#include "CUDAPoissonRRTKernels.h"
//#include <cuda.h>
#include <curand_kernel.h>
//#include <cuda_runtime.h>
#include "cuda_runtime.h"
#include "device_launch_parameters.h"
#include <curand.h>

#include "cuda_errorcheck.h"
#include "cuda_defs.h"
#include "cuda_reduceMin.h"
#include "collision/cuda_robobs.h"
#include "PlanningParams.h"
#include "cuda_statePrimitives.h"
#include "cuda_stateIntersect.h"
#include "cuda_timing.h"

// device memory random generator states
////////////////////////////////////////////////////////////////////////////////
curandState* d_randStates;
SerializedStates* d_startState;
SerializedStates* d_goalState;

// solution info
RRT_TREE_ID* d_solutionTreeId;
int* d_solutionNodeIndex;
RRT_TREE_ID h_solutionTreeId = NO_TREE;
RrtTree h_startTree;
RrtTree h_goalTree;

// device memory constants
////////////////////////////////////////////////////////////////////////////////
__device__ __constant__ float c_boundsMin[3];
__device__ __constant__ float c_boundsMax[3];
__device__ __constant__ float c_boundsDiff[3];
__device__ __constant__ float c_radius[MAX_LEVEL];

// device functions
////////////////////////////////////////////////////////////////////////////////


// device kernels
////////////////////////////////////////////////////////////////////////////////
__global__ void initializeRandomStatesKernel(int numStates, curandState* state)
{
	int id = threadIdx.y * blockDim.x + threadIdx.x;

	curandState* localState = state + id;
	curand_init(RANDOM_SEED, id, 0, localState);
}

__global__ void generateRandomStatesKernel(int numStates, curandState* state, SerializedStates* generatedStates)
{
	__shared__ float rotationalRandomValues[NUM_THREADS * 3];

	int id = threadIdx.y * blockDim.x + threadIdx.x;
	int stateIdx = threadIdx.x;
	int componentIdx = threadIdx.y;

	curandState* localState = state + id;

	// translational value
	float x = curand_uniform(localState);
	generatedStates[stateIdx].components.r[componentIdx] = x * c_boundsDiff[componentIdx] + c_boundsMin[componentIdx];

	// rotational value (quaternion)
	x = curand_uniform(localState);
	rotationalRandomValues[id] = x;
	__syncthreads();

	if (componentIdx < 2)
	{
		float x1 = rotationalRandomValues[stateIdx * 3];
		float c1 = sqrtf(1 - x1 + (-1 + 2 * x1) * componentIdx);
		float x2 = 2 * rotationalRandomValues[stateIdx * 3 + componentIdx + 1];
		float sinpix2, cospix2;
		sincospif(x2, &sinpix2, &cospix2);
		generatedStates[stateIdx].components.q[0 + componentIdx * 2] = sinpix2 * c1;
		generatedStates[stateIdx].components.q[1 + componentIdx * 2] = cospix2 * c1;
	}

	// debug
	/*
	if (componentIdx == 0)
	{
		printf("%d : %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", stateIdx, 
			generatedStates[stateIdx].components.r[0],
			generatedStates[stateIdx].components.r[1],
			generatedStates[stateIdx].components.r[2],
			generatedStates[stateIdx].components.q[0],
			generatedStates[stateIdx].components.q[1],
			generatedStates[stateIdx].components.q[2],
			generatedStates[stateIdx].components.q[3]);
	}
	*/
}

__global__ void reduceMin1Kernel(float *g_idata, float *g_odata, unsigned int query_width, int* g_oIndices, bool removeMin)
{
	extern __shared__ float sdata[];

	unsigned int queryIdx = blockIdx.y;
	unsigned int dataIdx = queryIdx;

	g_odata[queryIdx] = g_idata[queryIdx];
	g_oIndices[queryIdx] = 0;

	//printf("min : %f at %d is written to %x\n", g_idata[queryIdx], 0, g_oIndices + queryIdx);

	if (removeMin)
		g_idata[dataIdx] = FLOAT_MAX;
}

__global__ void reduceMin1WithIndicesKernel(float *g_idata, int* g_iIndices, float *g_odata, unsigned int n, unsigned int query_width, int* g_oIndices, bool removeMin)
{
	extern __shared__ float sdata[];

	unsigned int queryIdx = blockIdx.y;
	unsigned int refIdx = g_iIndices[queryIdx];
	unsigned int dataIdx = queryIdx * n + refIdx;

	g_odata[queryIdx] = g_idata[dataIdx];
	g_oIndices[queryIdx] = dataIdx;

	if (removeMin)
		g_idata[dataIdx] = FLOAT_MAX;
}

__global__ void computeDistanceMatrixKernel(SerializedStates* refStates, int numRefStates, SerializedStates* queryStates, int numQueryStates, float* outDist)
{
	__shared__ SerializedStates sharedQueryState;
	__shared__ SerializedStates sharedSquared[32];

	int queryIdx = blockIdx.y;
	int refIdx = blockIdx.x * 32 + threadIdx.y;
	int componentIdx = threadIdx.x;

	float q, r, x1, x2;

	if (refIdx < numRefStates)
	{
		// read from global memory
		if (threadIdx.y == 0)
		{
			sharedQueryState.x[componentIdx] = queryStates[queryIdx].x[componentIdx];
		}
		__syncthreads();

		q = sharedQueryState.x[componentIdx];
		r = refStates[refIdx].x[componentIdx];

		if (componentIdx < 3)
		{
			sharedSquared[threadIdx.y].x[componentIdx] = (q - r) * (q - r);
		}
		else
		{
			sharedSquared[threadIdx.y].x[componentIdx] = q * r;
		}
		__syncthreads();

		if (componentIdx == 0)
		{
			x1 = sqrtf(sharedSquared[threadIdx.y].components.r[0] + 
				sharedSquared[threadIdx.y].components.r[1] +
				sharedSquared[threadIdx.y].components.r[2]);
			x2 = fabs(sharedSquared[threadIdx.y].components.q[0] + 
				sharedSquared[threadIdx.y].components.q[1] + 
			sharedSquared[threadIdx.y].components.q[2] +
			sharedSquared[threadIdx.y].components.q[3]);
			x2 = (x2 > 1 - 1e-7) ? 0.0 : acosf(x2);
			outDist[queryIdx * numRefStates + refIdx] = x1 + x2;

			/*
			{
				printf("Dist btn (%.2f %.2f %.2f)%d (%.2f %.2f %.2f)%d : %.2f %.2f\n", 
					queryStates[queryIdx].x[0],
					queryStates[queryIdx].x[1],
					queryStates[queryIdx].x[2],
					queryIdx, 
					refStates[refIdx].x[0],
					refStates[refIdx].x[1],
					refStates[refIdx].x[2],
					refIdx, x1, x2);
			}
			*/
		}
	}
}

__global__ void computeDistancesFromTreeNodesToStatesKernel(int inNumQueries, SerializedStates* inTreeNodeStates, int* inTreeNodeIndices, 
	SerializedStates* inDestStates, float* outDistR3, float* outDistSO3)
{
	__shared__ int sharedNodeIndex[32];
	__shared__ SerializedStates sharedSrcState[32];
	__shared__ SerializedStates sharedDestState[32];
	__shared__ SerializedStates sharedSquared[32];

	int queryIdx = blockIdx.x * 32 + threadIdx.y;
	int componentIdx = threadIdx.x;
	int sharedIdx = threadIdx.y;

	float src, dest, x2;

	if (queryIdx < inNumQueries)
	{
		// read from global memory
		if (componentIdx == 0)
			sharedNodeIndex[threadIdx.y] = inTreeNodeIndices[queryIdx];
		__syncthreads();

		int localIdx = sharedNodeIndex[threadIdx.y];
		src = inTreeNodeStates[localIdx].x[componentIdx];
		sharedSrcState[sharedIdx].x[componentIdx] = src;
		dest = inDestStates[queryIdx].x[componentIdx];
		sharedDestState[sharedIdx].x[componentIdx] = dest;
		__syncthreads();

		if (componentIdx < 3)
		{
			sharedSquared[sharedIdx].x[componentIdx] = (dest - src) * (dest - src);
		}
		else
		{
			sharedSquared[sharedIdx].x[componentIdx] = src * dest;
		}
		__syncthreads();

		if (componentIdx == 0)
		{
			outDistR3[queryIdx] = sqrtf(sharedSquared[sharedIdx].components.r[0] + 
				sharedSquared[sharedIdx].components.r[1] + sharedSquared[sharedIdx].components.r[2]);
			
			x2 = fabs(sharedSquared[sharedIdx].components.q[0] + sharedSquared[sharedIdx].components.q[1] + 
				sharedSquared[sharedIdx].components.q[2] + sharedSquared[sharedIdx].components.q[3]);
			x2 = (x2 > 1 - 1e-7) ? 0.0 : acosf(x2);
			outDistSO3[queryIdx] = x2;

			/*
			printf("%d dist from (%.2f %.2f %.2f) to (%.2f %.2f %.2f) : (%f)\n", queryIdx,
				sharedSrcState[sharedIdx].components.r[0],
				sharedSrcState[sharedIdx].components.r[1],
				sharedSrcState[sharedIdx].components.r[2],
				sharedDestState[sharedIdx].components.r[0],
				sharedDestState[sharedIdx].components.r[1],
				sharedDestState[sharedIdx].components.r[2],
				outDistR3[queryIdx] + outDistSO3[queryIdx]);
				*/
		}
	}
}

__global__ void computeIntersectionStatesKernel(int inNumQueries, SerializedStates* inTreeNodeStates, int* inTreeNodeIndices, 
	int* intTreeNodeIndicesToSamples, int* inSampleLevels,
	SerializedStates* inDestStates, float* inDistR3, float* inDistSO3,
	SerializedStates* outItersectionStates)
{
	__shared__ int sharedNodeIndex[32];
	__shared__ SerializedStates sharedSrcState[32];
	__shared__ SerializedStates sharedDestState[32];
	__shared__ float sharedDistanceR3[32];
	__shared__ float sharedDistanceSO3[32];
	__shared__ float sharedRatio[32];
	__shared__ SerializedStates sharedSo3Inter[32];
	__shared__ float sharedSo3s0[32];
	__shared__ float sharedSo3s1[32];
	__shared__ float sharedSo3d[32];

	int queryIdx = blockIdx.x * 32 + threadIdx.y;
	int componentIdx = threadIdx.x;
	int sharedDataIdx = threadIdx.y;

	float src, dest;

	if (queryIdx < inNumQueries)
	{
		// read src/dest/distance from global memory
		if (componentIdx == 0)
		{
			sharedNodeIndex[sharedDataIdx] = inTreeNodeIndices[queryIdx];
			sharedDistanceR3[sharedDataIdx] = inDistR3[queryIdx];
			sharedDistanceSO3[sharedDataIdx] = inDistSO3[queryIdx];

			int level = inSampleLevels[intTreeNodeIndicesToSamples[sharedNodeIndex[sharedDataIdx]]];
			sharedRatio[sharedDataIdx] = c_radius[level] / (sharedDistanceR3[sharedDataIdx] + sharedDistanceSO3[sharedDataIdx]);
		}

		dest = inDestStates[queryIdx].x[componentIdx];
		sharedDestState[sharedDataIdx].x[componentIdx] = dest;
		__syncthreads();

		int localIdx = sharedNodeIndex[sharedDataIdx];
		src = inTreeNodeStates[localIdx].x[componentIdx];
		sharedSrcState[sharedDataIdx].x[componentIdx] = src;
		__syncthreads();

		// compute interpolation pt
		if (componentIdx < 3)
		{
			outItersectionStates[queryIdx].x[componentIdx] = src + (dest - src) * sharedRatio[sharedDataIdx];
		}
		else
		{
			// SO3 interpolation
			// compute length 
			if (componentIdx == 3)
			{
				float theta = sharedDistanceSO3[sharedDataIdx];
				sharedSo3d[sharedDataIdx] = 1.0 / sinf(theta);
				sharedSo3s0[sharedDataIdx] = sinf((1.0 - sharedRatio[sharedDataIdx]) * theta);
				sharedSo3s1[sharedDataIdx] = sinf(sharedRatio[sharedDataIdx] * theta);
			}

			sharedSo3Inter[sharedDataIdx].x[componentIdx] = src * dest;
			__syncthreads();

			if (componentIdx == 3)
			{
				float dq = sharedSo3Inter[sharedDataIdx].components.q[0] + sharedSo3Inter[sharedDataIdx].components.q[1] +
					sharedSo3Inter[sharedDataIdx].components.q[2] + sharedSo3Inter[sharedDataIdx].components.q[3];
				if (dq < 0)
					sharedSo3s1[sharedDataIdx] = -sharedSo3s1[sharedDataIdx];
			}
			__syncthreads();

			outItersectionStates[queryIdx].x[componentIdx] = (src * sharedSo3s0[sharedDataIdx] + dest * sharedSo3s1[sharedDataIdx]) * sharedSo3d[sharedDataIdx];
		}
		/*
		if (componentIdx == 0)
		{
			// TODO: debug
			__syncthreads();

			printf("%d intersection state from (%.2f %.2f %.2f) to (%.2f %.2f %.2f) : \n(%.2f %.2f %.2f)\n", queryIdx,
				sharedSrcState[sharedDataIdx].components.r[0],
				sharedSrcState[sharedDataIdx].components.r[1],
				sharedSrcState[sharedDataIdx].components.r[2],
				sharedDestState[sharedDataIdx].components.r[0],
				sharedDestState[sharedDataIdx].components.r[1],
				sharedDestState[sharedDataIdx].components.r[2],
				outItersectionStates[queryIdx].components.r[0],
				outItersectionStates[queryIdx].components.r[1],
				outItersectionStates[queryIdx].components.r[2]);
		}
		*/
	}
}

__global__ void computeDistancesToDestStatesKernel(int inNumQueries, int k, SerializedStates* inSampleStates, int* inSampleIndices, int* inSampleLevels,
	float* inDistancesFromIntersect, SerializedStates* inDestStates, float* outDistances, int* inTreeNodeToSampleIndices, int* inTreeNodeIndices)
{
	__shared__ SerializedStates sharedNeighbors[32];
	__shared__ SerializedStates sharedDest[32];
	__shared__ float sharedDistancesR3[32];
	__shared__ float sharedDistancesSO3[32];

	int queryId = blockIdx.x * 32 + threadIdx.y;
	int kId = blockIdx.y;
	int componentIdx = threadIdx.x;
	int sharedDataIdx = threadIdx.y;

	if (queryId < inNumQueries)
	{
		copyIndexedStatesFromGlobalToShared(inNumQueries, inSampleStates, inSampleIndices + inNumQueries * kId, sharedNeighbors);
		sharedDest[sharedDataIdx].x[componentIdx] = inDestStates[queryId].x[componentIdx];

		__syncthreads();

		computeStateDistances(sharedNeighbors, sharedDest, sharedDistancesR3, sharedDistancesSO3);

		if (threadIdx.x == 0)
		{
			float distToIntersect = inDistancesFromIntersect[kId * inNumQueries + queryId];
			float dist = sharedDistancesR3[threadIdx.y] + sharedDistancesSO3[threadIdx.y];
			if (distToIntersect > c_radius[inSampleLevels[inSampleIndices[kId * inNumQueries + queryId]]])
				dist = FLOAT_MAX;
			int srcSampleIndex = inTreeNodeToSampleIndices[inTreeNodeIndices[queryId]];
			if (srcSampleIndex == inSampleIndices[inNumQueries * kId + queryId])
			{
				// TODO: check
				dist = FLOAT_MAX;
			}
			outDistances[queryId * k + kId] = dist;
			/*
			printf("%d(%d) %d dist : %f\n", 
				queryId,
				(inSampleIndices + kId * inNumQueries)[queryId],
				kId,
				outDistances[queryId * k + kId]);
				*/
		}
	}
}

__global__ void computeIndexedIndicesKernel(int inNumQueries, int* inIndices, int* inIndicesOfIndices, int* outIndices)
{
	int queryIdx = blockIdx.x * 512 + threadIdx.x;
	
	if (queryIdx < inNumQueries)
	{
		outIndices[queryIdx] = inIndices[queryIdx + inIndicesOfIndices[queryIdx] * inNumQueries];
	}

	//printf("Index chosen for %d : %d\n", queryIdx, outIndices[queryIdx]);
}

__global__ void generateNewSamplesIfNoNeighborKernel(int inNumQueries, 
	int* numSamples, SerializedStates* samples, int* levels, RRT_TREE_ID* isTreeNode,
	float* inMinDistances, SerializedStates* inIntersectionStates, int* outSampleIndices,
	int* treeNodeIndices, int* inNodeIndices, SerializedStates* inDestStates)
{
	__shared__ float sharedDistances[32];
	__shared__ float sharedNewSampleId[32];

	int queryId = blockIdx.x * 32 + threadIdx.y;
	int sharedDataIdx = threadIdx.y;
	int componentIdx = threadIdx.x;

	if (queryId < inNumQueries)
	{
		if (threadIdx.x == 0)
		{
			sharedDistances[sharedDataIdx] = inMinDistances[queryId];
		}
		__syncthreads();

		// no neighbor -> add intersection state as a new node
		int newSampleId = -1;
		if (sharedDistances[sharedDataIdx] == FLOAT_MAX)
		{
			if (threadIdx.x == 0)
			{
				newSampleId = atomicAdd(numSamples, 1);
				outSampleIndices[queryId] = newSampleId;
				sharedNewSampleId[sharedDataIdx] = newSampleId;

				int nodeSampleId = treeNodeIndices[inNodeIndices[queryId]];
				int nodeLevel = levels[nodeSampleId];
				levels[newSampleId] = nodeLevel;

				isTreeNode[newSampleId] = NO_TREE;
			}
			__syncthreads();

			newSampleId = sharedNewSampleId[sharedDataIdx];

			//if (threadIdx.x < 3)
				samples[newSampleId].x[componentIdx] = inIntersectionStates[queryId].x[componentIdx];
			//else
				//samples[newSampleId].x[componentIdx] = inDestStates[queryId].x[componentIdx];

#ifdef DEBUG_NEW_SAMPLE
			if (threadIdx.x == 0)
			{
				__syncthreads();
				printf("%d New neighbor Sample(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)Lv %d \n", newSampleId,
					inIntersectionStates[queryId].x[0],
					inIntersectionStates[queryId].x[1],
					inIntersectionStates[queryId].x[2],
					inDestStates[queryId].x[3],
					inDestStates[queryId].x[4],
					inDestStates[queryId].x[5],
					inDestStates[queryId].x[6],
					levels[newSampleId]);
			}
#endif
		}
	}
}

__device__ void computeStateDistance(SerializedStates& fromState, SerializedStates& toState, float& distanceR3, float& distanceSO3)
{
	SerializedStates squared;

	squared.x[0] = toState.x[0] - fromState.x[0];
	squared.x[1] = toState.x[1] - fromState.x[1];
	squared.x[2] = toState.x[2] - fromState.x[2];
	squared.x[0] *= squared.x[0];
	squared.x[1] *= squared.x[1];
	squared.x[2] *= squared.x[2];
	
	squared.x[3] = fromState.x[3] * toState.x[3];
	squared.x[4] = fromState.x[4] * toState.x[4];
	squared.x[5] = fromState.x[5] * toState.x[5];
	squared.x[6] = fromState.x[6] * toState.x[6];

	float distR3 = sqrtf(squared.components.r[0] + squared.components.r[1] + squared.components.r[2]);
	float x2 = fabs(squared.components.q[0] + squared.components.q[1] +
		squared.components.q[2] + squared.components.q[3]);
	x2 = (x2 > 1 - 1e-7) ? 0.0 : acosf(x2);
	float distSO3 = x2;

	distanceR3 = distR3;
	distanceSO3 = distSO3;
}

__device__ void interpolateState(SerializedStates& fromState, SerializedStates& toState, float t, float& distanceSO3, SerializedStates& interpState)
{
	SerializedStates so3Inter;

	// compute interpolation pt of R3
	interpState.x[0] = fromState.x[0] * (1.f - t) + toState.x[0] * t;
	interpState.x[1] = fromState.x[1] * (1.f - t) + toState.x[1] * t;
	interpState.x[2] = fromState.x[2] * (1.f - t) + toState.x[2] * t;

	// SO3 interpolation
	so3Inter.x[3] = fromState.x[3] * toState.x[3];
	so3Inter.x[4] = fromState.x[4] * toState.x[4];
	so3Inter.x[5] = fromState.x[5] * toState.x[5];
	so3Inter.x[6] = fromState.x[6] * toState.x[6];

	float theta = distanceSO3;
	float so3d = 1.0 / sinf(theta);
	float so3s0 = sinf((1.0 - t) * theta);
	float so3s1 = sinf(t * theta);

	float dq = so3Inter.components.q[0] + so3Inter.components.q[1] +
		so3Inter.components.q[2] + so3Inter.components.q[3];
	if (dq < 0)
		so3s1 = -so3s1;

	interpState.x[3] = (fromState.x[3] * so3s0 + toState.x[3] * so3s1) * so3d;
	interpState.x[4] = (fromState.x[4] * so3s0 + toState.x[4] * so3s1) * so3d;
	interpState.x[5] = (fromState.x[5] * so3s0 + toState.x[5] * so3s1) * so3d;
	interpState.x[6] = (fromState.x[6] * so3s0 + toState.x[6] * so3s1) * so3d;
}

template <class CollideFunctor>
__global__ void checkCollisionKernel(int inNumQueries, SerializedStates* inTreeNodeStates, int* inTreeNodeIndices, 
	SerializedStates* inSampleStates, int* inSampleIndices,
	GMesh* robot, GMesh* obstacle, CollideFunctor collideF,
	int* outResult, SerializedStates* outColFreeStates)
{
	__shared__ SerializedStates sharedFromState;
	__shared__ SerializedStates sharedToState;
	__shared__ float sharedDistanceR3;
	__shared__ float sharedDistanceSO3;
	__shared__ SerializedStates sharedInterpolatedStates[COLLISION_THREADS];
	__shared__ int sharedResults[COLLISION_THREADS];

	int queryId = blockIdx.x;
	int sharedDataIdx = threadIdx.x;

	int N = blockDim.x;
	int i = threadIdx.x + 1;

	if (sharedDataIdx == 0)
	{
		sharedFromState = inTreeNodeStates[inTreeNodeIndices[queryId]];
		sharedToState = inSampleStates[inSampleIndices[queryId]];
		computeStateDistance(sharedFromState, sharedToState, sharedDistanceR3, sharedDistanceSO3);
	}
	__syncthreads();

	int result = COLLISION_FREE;
	float t = (float)i / N;
	SerializedStates interpolatedState;
	interpolateState(sharedFromState, sharedToState, t, sharedDistanceSO3, interpolatedState);

	/*
	if (componentIdx == 0 && sharedDataIdx == N - 1)
	{
		printf("Interp from (%.3f %.3f %.3f | %.3f %.3f %.3f %.3f) to (%.3f %.3f %.3f | %.3f %.3f %.3f %.3f) at %d :\n(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)\n",
			sharedFromState.x[0],
			sharedFromState.x[1],
			sharedFromState.x[2],
			sharedFromState.x[3],
			sharedFromState.x[4],
			sharedFromState.x[5],
			sharedFromState.x[6],
			sharedToState.x[0],
			sharedToState.x[1],
			sharedToState.x[2],
			sharedToState.x[3],
			sharedToState.x[4],
			sharedToState.x[5],
			sharedToState.x[6],
			sharedDataIdx,
			sharedInterpolatedStates[sharedDataIdx].x[0],
			sharedInterpolatedStates[sharedDataIdx].x[1],
			sharedInterpolatedStates[sharedDataIdx].x[2],
			sharedInterpolatedStates[sharedDataIdx].x[3],
			sharedInterpolatedStates[sharedDataIdx].x[4],
			sharedInterpolatedStates[sharedDataIdx].x[5],
			sharedInterpolatedStates[sharedDataIdx].x[6]);
	}
	*/

	result = collideF(robot->models, robot->vertexPointers, robot->triIdxPointers, &(interpolatedState.x[0]),
		obstacle->models, obstacle->vertexPointers, obstacle->triIdxPointers);

	result = (result == COLLISION_FREE) ? INT_MAX : sharedDataIdx;
	
	sharedResults[sharedDataIdx] = result;
	sharedInterpolatedStates[sharedDataIdx] = interpolatedState;
	__syncthreads();

	// parallel reduction to get min collision index
	if (sharedDataIdx < 32)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 32]);
	}
	if (sharedDataIdx < 16)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 16]);
	}
	if (sharedDataIdx < 8)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 8]);
	}
	if (sharedDataIdx < 4)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 4]);
	}
	if (sharedDataIdx < 2)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 2]);
	}
	if (sharedDataIdx < 1)
	{
		int firstColIdx = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 1]);

		if (firstColIdx == INT_MAX)
			result = COLLISION_FREE;
		else if (firstColIdx == 0)
			result = IN_COLLISION_WITH_NO_ADVANCE;
		else
		{
			result = IN_COLLISION;
			outColFreeStates[queryId] = sharedInterpolatedStates[firstColIdx - 1];
		}
		outResult[queryId] = result;

		/*
		printf("Collision Checking from (%.3f %.3f %.3f | %.3f %.3f %.3f %.3f) to (%.3f %.3f %.3f | %.3f %.3f %.3f %.3f) : %d\n",
			sharedFromState.x[0],
			sharedFromState.x[1],
			sharedFromState.x[2],
			sharedFromState.x[3],
			sharedFromState.x[4],
			sharedFromState.x[5],
			sharedFromState.x[6],
			sharedToState.x[0],
			sharedToState.x[1],
			sharedToState.x[2],
			sharedToState.x[3],
			sharedToState.x[4],
			sharedToState.x[5],
			sharedToState.x[6],
			result);
			*/
	}
}

__global__ void removeDuplicatedPairsKernel(int inNumQueries, int* inNodeIndices, int* inDestStateIndices, int* inResults)
{
	extern __shared__ int sharedNodeIndices[];
	int* sharedDestStateIndices = sharedNodeIndices + inNumQueries;
	int* sharedResults = sharedDestStateIndices + inNumQueries;

	int prevQuery = threadIdx.y;
	int curQuery = threadIdx.x;

	if (curQuery < inNumQueries)
	{
		if (threadIdx.y == 0)
		{
			sharedNodeIndices[threadIdx.x] = inNodeIndices[threadIdx.x];
			sharedDestStateIndices[threadIdx.x] = inDestStateIndices[threadIdx.x];
			sharedResults[threadIdx.x] = inResults[threadIdx.x];
		}
		__syncthreads();

		if (prevQuery < curQuery)
		{
			if (sharedNodeIndices[prevQuery] == sharedNodeIndices[curQuery] &&
				sharedDestStateIndices[prevQuery] == sharedDestStateIndices[curQuery])
				sharedResults[curQuery] = IN_COLLISION_WITH_NO_ADVANCE;
		}
		__syncthreads();

		if (threadIdx.y == 0)
			inResults[threadIdx.x] = sharedResults[threadIdx.x];
	}
}

__global__ void generateDestStatesForOtherTreeKernel(int inNumQueries, SerializedStates* samples,
	int* inDestStateIndices, int* inResults, SerializedStates* inColFreeStates, 
	SerializedStates* outStates)
{
	__shared__ int sharedResult[32];
	__shared__ int sharedDestStateIndices[32];

	int queryId = blockIdx.x * 32 + threadIdx.y;
	int componentIdx = threadIdx.x;
	int sharedDataIdx = threadIdx.y;

	if (queryId < inNumQueries)
	{
		if (threadIdx.x == 0)
		{
			sharedResult[sharedDataIdx] = inResults[queryId];
			sharedDestStateIndices[sharedDataIdx] = inDestStateIndices[queryId];
		}
		__syncthreads();

		int result = sharedResult[sharedDataIdx];

		if (result == IN_COLLISION)
		{
			// use collision free state
			outStates[queryId].x[componentIdx] = inColFreeStates[queryId].x[componentIdx];
		}
		else // COLLISION_FREE or IN_COLLISION_WITH_NO_ADVANCE
		{
			// use dest state
			int destStateIndex = sharedDestStateIndices[sharedDataIdx];
			outStates[queryId].x[componentIdx] = samples[destStateIndex].x[componentIdx];
		}
	}
}

__global__ void addNewNodesKernel(int inNumQueries, int* numTreeNodes, SerializedStates* treeNodes, 
	int* treeNodeIndices, int* treeNodeParentIndices, 
	int* numSamples, SerializedStates* samples, int* levels, RRT_TREE_ID* isTreeNode,
	int* inNodeIndices, int* inDestStateIndices, int* inResults, SerializedStates* inColFreeStates,
	SerializedStates* templateSamples, int numTemplateSamples,
	RRT_TREE_ID currentTreeId, RRT_TREE_ID otherTreeId,
	RRT_TREE_ID* d_solutionTreeId, int* d_solutionNodeIndex)
{
	__shared__ int sharedCollisionResult[32];
	__shared__ int sharedNewNodeId[32];
	__shared__ int sharedSrcTreeNodeId[32];
	__shared__ int sharedDestStateId[32];
	__shared__ SerializedStates sharedFromState[32];
	__shared__ SerializedStates sharedToState[32];
	__shared__ SerializedStates sharedColFreeState[32];
	__shared__ int sharedDestStateLevel[32];
	__shared__ int sharedSrcStateLevel[32];

	int queryId = blockIdx.x * 32 + threadIdx.y;
	int componentIdx = threadIdx.x;
	int sharedDataIdx = threadIdx.y;

	if (queryId < inNumQueries)
	{
		int result = COLLISION_FREE;
		int destStateId = -1;

		int srcTreeNodeId = -1;
		int srcStateId = -1;

		// copy from global to shared
		if (threadIdx.x == 0)
		{
			result = inResults[queryId];
			sharedCollisionResult[sharedDataIdx] = result;

			destStateId = inDestStateIndices[queryId];
			sharedDestStateId[sharedDataIdx] = destStateId;

			srcTreeNodeId = inNodeIndices[queryId];
			sharedSrcTreeNodeId[sharedDataIdx] = srcTreeNodeId;

			if (result == COLLISION_FREE)
			{
				if (isTreeNode[destStateId] & currentTreeId)
					sharedNewNodeId[sharedDataIdx] = -1;
				else
					sharedNewNodeId[sharedDataIdx] = atomicAdd(numTreeNodes, 1);
			}
			if (result == IN_COLLISION)
			{
				sharedDestStateLevel[sharedDataIdx] = levels[destStateId];

				srcStateId = treeNodeIndices[srcTreeNodeId];
				sharedSrcStateLevel[sharedDataIdx] = levels[srcStateId];
			}
		}
		__syncthreads();

		result = sharedCollisionResult[sharedDataIdx];
		destStateId = sharedDestStateId[sharedDataIdx];
		srcTreeNodeId = sharedSrcTreeNodeId[sharedDataIdx];
		
		if (result == COLLISION_FREE)
		{
			// add new node to the tree
			int newNodeId = sharedNewNodeId[sharedDataIdx];
			if (newNodeId != -1)
			{
				if (threadIdx.x == 0)
				{
					treeNodeIndices[newNodeId] = destStateId;
					treeNodeParentIndices[newNodeId] = srcTreeNodeId;
					// TODO: CAS?
					//int treeId = atomicCAS((int*)&isTreeNode[destStateId], (int)otherTreeId, (int)BOTH_TREE);
					if (isTreeNode[destStateId] == otherTreeId)
					//if (treeId == otherTreeId)
					{
						isTreeNode[destStateId] = BOTH_TREE;

						// solution
						*d_solutionTreeId = currentTreeId;
						*d_solutionNodeIndex = newNodeId;
					}
					else
						isTreeNode[destStateId] = currentTreeId;
				}
				treeNodes[newNodeId].x[componentIdx] = samples[destStateId].x[componentIdx];

#ifdef DEBUG_NEW_NODE	
				if (threadIdx.x == 0 && currentTreeId == 1)
				{
					printf("Tree-%d %d-%d : Node(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)Lv %d\n", currentTreeId, sharedSrcTreeNodeId[sharedDataIdx], sharedNewNodeId[sharedDataIdx],
						samples[destStateId].x[0],
						samples[destStateId].x[1],
						samples[destStateId].x[2],
						samples[destStateId].x[3],
						samples[destStateId].x[4],
						samples[destStateId].x[5],
						samples[destStateId].x[6],
						levels[destStateId]);
				}
#endif
			}
		}
		if (result == IN_COLLISION)
		{
			// apply adaptive sampling
			sharedFromState[sharedDataIdx].x[componentIdx] = treeNodes[srcTreeNodeId].x[componentIdx];
			sharedToState[sharedDataIdx].x[componentIdx] = samples[destStateId].x[componentIdx];
			sharedColFreeState[sharedDataIdx].x[componentIdx] = inColFreeStates[queryId].x[componentIdx];

			int srcLevel = sharedSrcStateLevel[sharedDataIdx];
			int destLevel = sharedDestStateLevel[sharedDataIdx];

			if (subdivideDisk(sharedToState, sharedColFreeState, templateSamples, numTemplateSamples, destLevel, c_radius[destLevel], 
				numSamples, samples, levels, isTreeNode))
			{
				if (threadIdx.x == 0)
				{
					levels[destStateId] = destLevel + 1;
				}
			}
			else if (subdivideDisk(sharedFromState, sharedColFreeState, templateSamples, numTemplateSamples, srcLevel, c_radius[srcLevel], 
				numSamples, samples, levels, isTreeNode))
			{
				if (threadIdx.x == 0)
				{
					levels[srcStateId] = srcLevel + 1;
				}
			}
		}
	}
}

__global__ void addNewNodesKernelS(int inNumQueries, int* numTreeNodes, SerializedStates* treeNodes, 
	int* treeNodeIndices, int* treeNodeParentIndices, 
	int* numSamples, SerializedStates* samples, int* levels, RRT_TREE_ID* isTreeNode,
	int* inNodeIndices, int* inDestStateIndices, int* inResults, SerializedStates* inColFreeStates,
	SerializedStates* templateSamples, int numTemplateSamples,
	RRT_TREE_ID currentTreeId, RRT_TREE_ID otherTreeId,
	RRT_TREE_ID* d_solutionTreeId, int* d_solutionNodeIndex, curandState* randState)
{
	int queryId = blockIdx.x * 32 + threadIdx.x;

	if (queryId < inNumQueries)
	{
		// copy from global
		int result = inResults[queryId];
		int destStateId = inDestStateIndices[queryId];
		int srcTreeNodeId = inNodeIndices[queryId];
		
		int srcStateId = -1;
		int destStateLevel = -1;
		int srcStateLevel = -1;

		if (result == COLLISION_FREE)
		{
			int newNodeId = -1;
			if ((isTreeNode[destStateId] & currentTreeId) == 0)
				newNodeId = atomicAdd(numTreeNodes, 1);

			if (newNodeId != -1)
			{
				treeNodeIndices[newNodeId] = destStateId;
				treeNodeParentIndices[newNodeId] = srcTreeNodeId;
				// TODO: CAS?
				//int treeId = atomicCAS((int*)&isTreeNode[destStateId], (int)otherTreeId, (int)BOTH_TREE);
				if (isTreeNode[destStateId] == otherTreeId)
					//if (treeId == otherTreeId)
				{
					isTreeNode[destStateId] = BOTH_TREE;

					// solution
					*d_solutionTreeId = currentTreeId;
					*d_solutionNodeIndex = newNodeId;
				}
				else
					isTreeNode[destStateId] = currentTreeId;
		
				treeNodes[newNodeId] = samples[destStateId];

#ifdef DEBUG_NEW_NODE	
				if (currentTreeId == 1)
				{
					printf("Tree-%d %d-%d : Node(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)Lv %d\n", currentTreeId, srcTreeNodeId, newNodeId,
						samples[destStateId].x[0],
						samples[destStateId].x[1],
						samples[destStateId].x[2],
						samples[destStateId].x[3],
						samples[destStateId].x[4],
						samples[destStateId].x[5],
						samples[destStateId].x[6],
						levels[destStateId]);
				}
#endif
			}
		}
		if (result == IN_COLLISION)
		{
			destStateLevel = levels[destStateId];
			srcStateId = treeNodeIndices[srcTreeNodeId];
			srcStateLevel = levels[srcStateId];

			// apply adaptive sampling
			SerializedStates fromState = treeNodes[srcTreeNodeId];
			SerializedStates toState = samples[destStateId];
			SerializedStates colFreeState = inColFreeStates[queryId];

			if (subdivideDiskS(toState, colFreeState, templateSamples, numTemplateSamples, destStateLevel, c_radius[destStateLevel], 
				numSamples, samples, levels, isTreeNode, randState[queryId]))
			{
				levels[destStateId] = destStateLevel + 1;
			}
			else if (subdivideDiskS(fromState, colFreeState, templateSamples, numTemplateSamples, srcStateLevel, c_radius[srcStateLevel], 
				numSamples, samples, levels, isTreeNode, randState[queryId]))
			{
				levels[srcStateId] = srcStateLevel + 1;
			}
		}
	}
}

////////////////////////////////////////////////////////////////////////////////
namespace poisson
{

namespace cuda
{

void initializeMemory(PoissonDiskSampleData& d_sampleData, GMesh** d_robot, GMesh** d_obstacle, RrtTree& d_startTree, RrtTree& d_goalTree)
{
	CUDA_CHECK_ERROR();

	// precomputed PD samples
	GPUMALLOC(&d_sampleData.samples, sizeof(SerializedStates) * MAX_SAMPLES);
	GPUMALLOC(&d_sampleData.numSamples, sizeof(int));
	CPUMALLOC(&d_sampleData.h_numSamples, sizeof(int));
	CUDA_CHECK_ERROR();
	//cudaHostRegister(d_sampleData.h_numSamples, sizeof(int), 0);
	CUDA_CHECK_ERROR();
	GPUMALLOC(&d_sampleData.levels, sizeof(int) * MAX_SAMPLES);
	GPUMALLOC(&d_sampleData.isTreeNode, sizeof(RRT_TREE_ID) * MAX_SAMPLES);

	CUDA_CHECK_ERROR();

	// meshes
	GPUMALLOC(d_robot, sizeof(GMesh));
	GPUMALLOC(d_obstacle, sizeof(GMesh));

	// random states
	GPUMALLOC(&d_randStates, sizeof(curandState) * NUM_THREADS * 3);

	CUDA_CHECK_ERROR();

	// start tree
	GPUMALLOC(&d_startTree.numTreeNodes, sizeof(int));
	CPUMALLOC(&d_startTree.h_numTreeNodes, sizeof(int));
	//cudaHostRegister(d_startTree.h_numTreeNodes, sizeof(int), 0);
	GPUMALLOC(&d_startTree.treeNodeIndices, sizeof(int) * MAX_NODES);
	GPUMALLOC(&d_startTree.treeNodeParentIndices, sizeof(int) * MAX_NODES);
	GPUMALLOC(&d_startTree.treeNodes, sizeof(SerializedStates) * MAX_NODES);
	d_startState = d_startTree.treeNodes;
	d_startTree.h_RrtTreeId = START_TREE;

	// goal tree
	GPUMALLOC(&d_goalTree.numTreeNodes, sizeof(int));
	CPUMALLOC(&d_goalTree.h_numTreeNodes, sizeof(int));
	//cudaHostRegister(d_goalTree.h_numTreeNodes, sizeof(int), 0);
	GPUMALLOC(&d_goalTree.treeNodeIndices, sizeof(int) * MAX_NODES);
	GPUMALLOC(&d_goalTree.treeNodeParentIndices, sizeof(int) * MAX_NODES);
	GPUMALLOC(&d_goalTree.treeNodes, sizeof(SerializedStates) * MAX_NODES);
	d_goalState = d_goalTree.treeNodes;
	d_goalTree.h_RrtTreeId = GOAL_TREE;

	CUDA_CHECK_ERROR();

	// termination check
	GPUMALLOC(&d_solutionTreeId, sizeof(RRT_TREE_ID));
	GPUMALLOC(&d_solutionNodeIndex, sizeof(int));
	//cudaHostRegister(&h_solutionTreeId, sizeof(int), 0);

	// solution retrieve
	CPUMALLOC(&h_startTree.treeNodeIndices, sizeof(int) * MAX_NODES);
	CPUMALLOC(&h_startTree.treeNodeParentIndices, sizeof(int) * MAX_NODES);
	CPUMALLOC(&h_startTree.treeNodes, sizeof(SerializedStates) * MAX_NODES);
	CPUMALLOC(&h_goalTree.treeNodeIndices, sizeof(int) * MAX_NODES);
	CPUMALLOC(&h_goalTree.treeNodeParentIndices, sizeof(int) * MAX_NODES);
	CPUMALLOC(&h_goalTree.treeNodes, sizeof(SerializedStates) * MAX_NODES);

	CUDA_CHECK_ERROR();
}

void uploadData(const std::vector<float>& vBoundsMin, const std::vector<float>& vBoundsMax, 
	const GMesh* h_robot, const GMesh* h_obstacle, const std::vector<SerializedStates>& vSerializedStates, 
	const std::vector<SerializedStates>& vTemplateStates, int numPrecomputedPDSamples,
	GMesh* d_robot, GMesh* d_obstacle, PoissonDiskSampleData& d_sampleData, PoissonDiskSampleData& d_templateData)
{
	CUDA_CHECK_ERROR();

	// precomputed PD samples
	TOGPU(d_sampleData.samples, &(vSerializedStates[0]), sizeof(SerializedStates) * numPrecomputedPDSamples);
	GPUMEMSET(d_sampleData.levels, 0, sizeof(int) * MAX_SAMPLES);
	GPUMEMSET(d_sampleData.isTreeNode, 0, sizeof(RRT_TREE_ID) * MAX_SAMPLES);
	TOGPU(d_sampleData.numSamples, &numPrecomputedPDSamples, sizeof(int));
	*(d_sampleData.h_numSamples) = numPrecomputedPDSamples;

	// precomputed adaptive template states
	// allocate here!
	int numTemplateSamples = vTemplateStates.size();
	GPUMALLOC(&d_templateData.samples, sizeof(SerializedStates) * numTemplateSamples);
	GPUMALLOC(&d_templateData.numSamples, sizeof(int));
	CPUMALLOC(&d_templateData.h_numSamples, sizeof(int));
	GPUMALLOC(&d_templateData.levels, sizeof(int) * numTemplateSamples);
	GPUMALLOC(&d_templateData.isTreeNode, sizeof(RRT_TREE_ID) * numTemplateSamples);
	// copy data
	TOGPU(d_templateData.samples, &(vTemplateStates[0]), sizeof(SerializedStates) * numTemplateSamples);
	GPUMEMSET(d_templateData.levels, 0, sizeof(int) * numTemplateSamples);
	GPUMEMSET(d_templateData.isTreeNode, 0, sizeof(RRT_TREE_ID) * numTemplateSamples);
	TOGPU(d_templateData.numSamples, &numTemplateSamples, sizeof(int));
	*(d_templateData.h_numSamples) = numTemplateSamples;

	// bounds
	TOGPU_CONSTANT(c_boundsMin, &(vBoundsMin[0]), sizeof(float) * 3);
	TOGPU_CONSTANT(c_boundsMax, &(vBoundsMax[0]), sizeof(float) * 3);
	std::vector<float> vBoundsDiff(3);
	for (int i = 0; i < 3; ++i)
		vBoundsDiff[i] = vBoundsMax[i] - vBoundsMin[i];
	TOGPU_CONSTANT(c_boundsDiff, &(vBoundsDiff[0]), sizeof(float) * 3);

	// radii
	std::vector<float> vRadius(MAX_LEVEL);
	vRadius[0] = BASE_RADIUS;
	for (int i = 1; i < MAX_LEVEL; ++i)
		vRadius[i] = vRadius[i - 1] * 0.5f;
	TOGPU_CONSTANT(c_radius, &(vRadius[0]), sizeof(float) * MAX_LEVEL);

	// meshes
	TOGPU(d_robot, h_robot, sizeof(GMesh));
	TOGPU(d_obstacle, h_obstacle, sizeof(GMesh));

	// termination check
	TOGPU(d_solutionTreeId, &h_solutionTreeId, sizeof(RRT_TREE_ID));
	int h_solutionNodeIndex = -1;
	TOGPU(d_solutionNodeIndex, &h_solutionNodeIndex, sizeof(int));

	CUDA_CHECK_ERROR();
}

void initializeRandomStates()
{
	dim3 grids = dim3(1, 1, 1);
	dim3 threads = dim3(NUM_THREADS, 3, 1);
	initializeRandomStatesKernel <<< grids, threads >>> (NUM_THREADS, d_randStates);

	CUDA_CHECK_ERROR();
}

void freeMemory(PoissonDiskSampleData& d_sampleData, PoissonDiskSampleData& d_templateData, 
	GMesh* d_robot, GMesh* d_obstacle, RrtTree& d_startTree, RrtTree& d_goalTree)
{
	// precomputed PD samples
	GPUFREE(d_sampleData.numSamples);
	//cudaHostUnregister(d_sampleData.h_numSamples);
	CPUFREE(d_sampleData.h_numSamples);
	GPUFREE(d_sampleData.samples);
	GPUFREE(d_sampleData.levels);
	GPUFREE(d_sampleData.isTreeNode);

	// precomputed adaptive template states
	GPUFREE(d_templateData.numSamples);
	CPUFREE(d_templateData.h_numSamples);
	GPUFREE(d_templateData.samples);
	GPUFREE(d_templateData.levels);
	GPUFREE(d_templateData.isTreeNode);

	// meshes
	GPUMALLOC(&d_robot, sizeof(GMesh));
	GPUMALLOC(&d_obstacle, sizeof(GMesh));

	// random states
	GPUFREE(d_randStates);

	// start tree
	GPUFREE(d_startTree.numTreeNodes);
	//cudaHostUnregister(d_startTree.h_numTreeNodes);
	CPUFREE(d_startTree.h_numTreeNodes);
	GPUFREE(d_startTree.treeNodeIndices);
	GPUFREE(d_startTree.treeNodeParentIndices);
	GPUFREE(d_startTree.treeNodes);

	// goal tree
	GPUFREE(d_goalTree.numTreeNodes);
	//cudaHostUnregister(d_goalTree.h_numTreeNodes);
	CPUFREE(d_goalTree.h_numTreeNodes);
	GPUFREE(d_goalTree.treeNodeIndices);
	GPUFREE(d_goalTree.treeNodeParentIndices);
	GPUFREE(d_goalTree.treeNodes);

	// termination check
	GPUFREE(d_solutionTreeId);
	GPUFREE(d_solutionNodeIndex);
	//cudaHostUnregister(&h_solutionTreeId);

	// solution retrieve
	CPUFREE(h_startTree.treeNodeIndices);
	CPUFREE(h_startTree.treeNodeParentIndices);
	CPUFREE(h_startTree.treeNodes);
	CPUFREE(h_goalTree.treeNodeIndices);
	CPUFREE(h_goalTree.treeNodeParentIndices);
	CPUFREE(h_goalTree.treeNodes);
}

void setStartAndGoalStates(SerializedStates* h_startAndGoalStates, int numPrecomputedPDSamples, PoissonDiskSampleData& d_sampleData,
	int& numSamples, RrtTree& d_startTree, RrtTree& d_goalTree)
{
	TOGPU(d_sampleData.samples + numPrecomputedPDSamples, h_startAndGoalStates, sizeof(SerializedStates) * 2);
	GPUTOGPU(d_startState, d_sampleData.samples + numPrecomputedPDSamples, sizeof(SerializedStates));
	GPUTOGPU(d_goalState, d_sampleData.samples + (numPrecomputedPDSamples + 1), sizeof(SerializedStates));

	const RRT_TREE_ID isTreeNode[2] = {START_TREE, GOAL_TREE};
	TOGPU(d_sampleData.isTreeNode + numPrecomputedPDSamples, isTreeNode, sizeof(RRT_TREE_ID) * 2);

	TOGPU(d_startTree.treeNodeIndices, &numSamples, sizeof(int));
	++numSamples;
	TOGPU(d_goalTree.treeNodeIndices, &numSamples, sizeof(int));
	++numSamples;

	const int noParent = -1;
	TOGPU(d_startTree.treeNodeParentIndices, &noParent, sizeof(int));
	TOGPU(d_goalTree.treeNodeParentIndices, &noParent, sizeof(int));

	unsigned int numStartTreeNodes = 1;
	unsigned int numGoalTreeNodes = 1;

	TOGPU(d_sampleData.numSamples, &numSamples, sizeof(unsigned int));
	*(d_sampleData.h_numSamples) = numSamples;
	TOGPU(d_startTree.numTreeNodes, &numStartTreeNodes, sizeof(int));
	TOGPU(d_goalTree.numTreeNodes, &numGoalTreeNodes, sizeof(int));
	*(d_startTree.h_numTreeNodes) = numStartTreeNodes;
	*(d_goalTree.h_numTreeNodes) = numGoalTreeNodes;

	CUDA_CHECK_ERROR();

#ifdef DEBUG_NEW_NODE	
	printf("Start Node(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)\n",
		h_startAndGoalStates[0].x[0],
		h_startAndGoalStates[0].x[1],
		h_startAndGoalStates[0].x[2],
		h_startAndGoalStates[0].x[3],
		h_startAndGoalStates[0].x[4],
		h_startAndGoalStates[0].x[5],
		h_startAndGoalStates[0].x[6]);
	printf("Goal Node(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)\n",
		h_startAndGoalStates[1].x[0],
		h_startAndGoalStates[1].x[1],
		h_startAndGoalStates[1].x[2],
		h_startAndGoalStates[1].x[3],
		h_startAndGoalStates[1].x[4],
		h_startAndGoalStates[1].x[5],
		h_startAndGoalStates[1].x[6]);
#endif
}

void generateRandomStates(SerializedStates* generatedStates)
{
	dim3 grids = dim3(1, 1, 1);
	dim3 threads = dim3(NUM_THREADS, 3, 1);

	CUDA_TIMING_BEGIN();

	generateRandomStatesKernel <<< grids, threads >>> (NUM_THREADS, d_randStates, generatedStates);

	CUDA_TIMING_END("generateRandomStatesKernel");

	CUDA_CHECK_ERROR();
}

void knn(SerializedStates* inRefStates, int inNumRefStates, SerializedStates* inQueryStates, int numQueryStates, unsigned int k, 
	float* internalDistances, float* outDistances, int* outIndices)
{
	int gridX = (inNumRefStates + 31) / 32;
	dim3 grids = dim3(gridX, numQueryStates, 1);
	dim3 threads = dim3(7, 32, 1);

	CUDA_TIMING_BEGIN();

	computeDistanceMatrixKernel <<< grids, threads >>> (inRefStates, inNumRefStates, inQueryStates, numQueryStates, internalDistances);

	CUDA_CHECK_ERROR();

	float* distWrite = outDistances;
	int* indWrite = outIndices;
	for (unsigned int i = 0; i < k; ++i)
	{
		if (inNumRefStates > 512 * 512)
		{
			// TODO: 3-level
		}
		else if (inNumRefStates > 512)
		{
			int blockSize = 512;
			gridX = (inNumRefStates + (blockSize-1)) / blockSize;
			grids = dim3(gridX, numQueryStates, 1);
			threads = dim3(blockSize, 1, 1);
			int smemSize = blockSize * (sizeof(float) + sizeof(int)) * 2;

			float* distWrite2 = distWrite + numQueryStates * gridX;
			int* indWrite2 = indWrite + numQueryStates * gridX;
			reduceMin<256, float><<< grids, threads, smemSize >>>(internalDistances, distWrite2, inNumRefStates, numQueryStates, indWrite2, false);

			// TODO: test
			/*
			float h_result[2 * (4 + 512)];
			int h_resultIndices[2 * (4 + 512)];
			FROMGPU(h_result, outDistances, sizeof(float) * numQueryStates * (k + 512));
			FROMGPU(h_resultIndices, outIndices, sizeof(int) * numQueryStates * (k + 512));
			*/

			CUDA_CHECK_ERROR();

			// distWrite2, indWrite2 has gridX * query_width results;

			int nextStepWidth = gridX;
			while (blockSize / 2 >= nextStepWidth)
			{
				blockSize /= 2;
			}
			gridX = (nextStepWidth + (blockSize-1)) / blockSize;
			grids = dim3(gridX, numQueryStates, 1);
			threads = dim3(blockSize, 1, 1);
			smemSize = blockSize * (sizeof(float) + sizeof(int)) * 2;

			switch (blockSize)
			{
			case 512:
				reduceMinWithIndices<256, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 256:
				reduceMinWithIndices<128, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 128:
				reduceMinWithIndices< 64, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 64:
				reduceMinWithIndices< 32, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 32:
				reduceMinWithIndices< 16, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 16:
				reduceMinWithIndices< 8, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 8:
				reduceMinWithIndices< 4, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 4:
				reduceMinWithIndices< 2, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 2:
				reduceMinWithIndices< 1, float><<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, nextStepWidth, numQueryStates, indWrite, true); break;
			case 1:
				reduceMin1WithIndicesKernel<<< grids, threads, smemSize >>>(internalDistances, indWrite2, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			}

			// TODO: test
			/*
			FROMGPU(h_result, outDistances, sizeof(float) * numQueryStates * (k + 512));
			FROMGPU(h_resultIndices, outIndices, sizeof(int) * numQueryStates * (k + 512));
			*/

			CUDA_CHECK_ERROR();
		}
		else
		{
			int blockSize = 512;
			while (blockSize / 2 >= inNumRefStates)
			{
				blockSize /= 2;
			}
			gridX = (inNumRefStates + (blockSize-1)) / blockSize;
			grids = dim3(gridX, numQueryStates, 1);
			threads = dim3(blockSize, 1, 1);
			int smemSize = blockSize * (sizeof(float) + sizeof(int)) * 2;

			switch (blockSize)
			{
			case 512:
				reduceMin<256, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 256:
				reduceMin<128, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 128:
				reduceMin< 64, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 64:
				reduceMin< 32, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 32:
				reduceMin< 16, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 16:
				reduceMin< 8, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 8:
				reduceMin< 4, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 4:
				reduceMin< 2, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 2:
				reduceMin< 1, float><<< grids, threads, smemSize >>>(internalDistances, distWrite, inNumRefStates, numQueryStates, indWrite, true); break;
			case 1:
				reduceMin1Kernel<<< grids, threads, smemSize >>>(internalDistances, distWrite, numQueryStates, indWrite, true); break;
			}
			CUDA_CHECK_ERROR();
		}
		distWrite += numQueryStates;
		indWrite += numQueryStates;
	}

	CUDA_TIMING_END("knn");

	CUDA_CHECK_ERROR();
}

void computeDistancesFromTreeNodesToStates(int inNumQueries, SerializedStates* inTreeNodeStates, int* inTreeNodeIndices, 
	SerializedStates* inDestStates, float* outDistR3, float* outDistSO3)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, 1, 1);
	dim3 threads = dim3(7, 32, 1);

	CUDA_TIMING_BEGIN();

	computeDistancesFromTreeNodesToStatesKernel <<< grids, threads >>> (inNumQueries, inTreeNodeStates, inTreeNodeIndices, 
		inDestStates, outDistR3, outDistSO3);

	CUDA_TIMING_END("computeDistancesFromTreeNodesToStatesKernel");

	CUDA_CHECK_ERROR();
}

void computeIntersectionStates(int inNumQueries, SerializedStates* inTreeNodeStates, int* inTreeNodeIndices, 
	int* intTreeNodeIndicesToSamples, int* inSampleLevels,
	SerializedStates* inDestStates, float* inDistR3, float* inDistSO3,
	SerializedStates* outItersectionStates)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, 1, 1);
	dim3 threads = dim3(7, 32, 1);

	CUDA_TIMING_BEGIN();

	computeIntersectionStatesKernel <<< grids, threads >>> (inNumQueries, inTreeNodeStates, inTreeNodeIndices, 
		intTreeNodeIndicesToSamples, inSampleLevels,
		inDestStates, inDistR3, inDistSO3, 
		outItersectionStates);

	CUDA_TIMING_END("computeIntersectionStatesKernel");

	CUDA_CHECK_ERROR();
}

void computeDistancesToDestStates(int inNumQueries, int k, SerializedStates* inSampleStates, int* inSampleIndices, int* inSampleLevels,
	float* inDistancesFromIntersect, SerializedStates* indestStates, float* outDistances, int* inTreeNodeToSampleIndices, int* inTreeNodeIndices)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, k, 1);
	dim3 threads = dim3(7, 32, 1);

	CUDA_TIMING_BEGIN();

	computeDistancesToDestStatesKernel <<< grids, threads >>> (inNumQueries, k, inSampleStates, inSampleIndices, inSampleLevels, 
		inDistancesFromIntersect, indestStates, outDistances, inTreeNodeToSampleIndices, inTreeNodeIndices);

	CUDA_TIMING_END("computeDistancesToDestStatesKernel");

	CUDA_CHECK_ERROR();
}

void computeMinDistanceSamples(int inNumQueries, int k, SerializedStates* inSampleStates, int* inSampleIndices, 
	float* inDistances, int* outSampleIndices, float* outDistances)
{
	CUDA_TIMING_BEGIN();

	int blockSize = 512;
	while (blockSize / 2 >= k)
	{                                                                        
		blockSize /= 2;
	}
	int gridX = (k + (blockSize-1)) / blockSize;
	dim3 grids = dim3(gridX, inNumQueries, 1);
	dim3 threads = dim3(blockSize, 1, 1);
	int smemSize = blockSize * (sizeof(float) + sizeof(int));

	switch (blockSize)
	{
	case 512:
		reduceMin<256, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 256:
		reduceMin<128, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 128:
		reduceMin< 64, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 64:
		reduceMin< 32, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 32:
		reduceMin< 16, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 16:
		reduceMin< 8, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 8:
		reduceMin< 4, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 4:
		reduceMin< 2, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 2:
		reduceMin< 1, float><<< grids, threads, smemSize >>>(inDistances, outDistances, k, inNumQueries, outSampleIndices, false); break;
	case 1:
		reduceMin1Kernel<<< grids, threads, smemSize >>>(inDistances, outDistances, inNumQueries, outSampleIndices, false); break;
	}

	{
		int gridX = (inNumQueries + (512-1)) / 512;
		dim3 grids = dim3(gridX, 1, 1);
		dim3 threads = dim3(min(inNumQueries, 512), 1, 1);

		computeIndexedIndicesKernel <<< grids, threads >>>(inNumQueries, inSampleIndices, outSampleIndices, outSampleIndices);
	}

	CUDA_TIMING_END("computeMinDistanceSamples");

	CUDA_CHECK_ERROR();
}

void generateNewSamplesIfNoNeighbor(int inNumQueries, PoissonDiskSampleData& d_sampleData, 
	float* inMinDistances, SerializedStates* inIntersectionStates, int* outSampleIndices,
	int* treeNodeIndices, int* inNodeIndices, SerializedStates* inDestStates)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, 1, 1);
	dim3 threads = dim3(7, 32, 1);

	CUDA_TIMING_BEGIN();

	generateNewSamplesIfNoNeighborKernel <<< grids, threads >>> (inNumQueries, 
		d_sampleData.numSamples, d_sampleData.samples, d_sampleData.levels, d_sampleData.isTreeNode,
		inMinDistances, inIntersectionStates, outSampleIndices,
		treeNodeIndices, inNodeIndices, inDestStates);

	CUDA_TIMING_END("generateNewSamplesIfNoNeighborKernel");

	CUDA_CHECK_ERROR();
}

void checkCollision(int inNumQueries, SerializedStates* inTreeNodeStates, int* inTreeNodeIndices, 
	SerializedStates* inSampleStates, int* inSampleIndices,
	int* outResult, SerializedStates* outColFreeStates,
	GMesh* d_robot, GMesh* d_obstacle,
	float stateValidityCheckingResolutionR3, float stateValidityCheckingResolutionSO3)
{
	dim3 grids = dim3(inNumQueries, 1, 1);
	dim3 threads = dim3(COLLISION_THREADS, 1, 1);

	CUDA_TIMING_BEGIN();

	// compute N (# of checking points)
	BVHStateCollideFunctor<OBBNode, OBB, ushort2, IN_COLLISION, unsigned short> collideF;
	checkCollisionKernel<BVHStateCollideFunctor<OBBNode, OBB, ushort2, IN_COLLISION, unsigned short> > <<< grids, threads >>>(
		inNumQueries, inTreeNodeStates, inTreeNodeIndices, inSampleStates, inSampleIndices, 
		d_robot, d_obstacle, collideF,
		outResult, outColFreeStates);

	CUDA_TIMING_END("checkCollisionKernel");

	CUDA_CHECK_ERROR();
}

void removeDuplicatedPairs(int inNumQueries, int* inNodeIndices, int* inDestStateIndices, int* inResults)
{
	// TODO: large iNumQueries?
	dim3 grids = dim3(1, 1, 1);
	dim3 threads = dim3(inNumQueries, inNumQueries, 1);
	int smemSize = sizeof(int) * inNumQueries * 3;

	CUDA_TIMING_BEGIN();

	removeDuplicatedPairsKernel <<< grids, threads, smemSize >>> (inNumQueries, inNodeIndices, inDestStateIndices, inResults);

	CUDA_TIMING_END("removeDuplicatedPairsKernel");

	CUDA_CHECK_ERROR();
}

void addNewNodes(int inNumQueries, RrtTree& currentTree, RrtTree& otherTree, PoissonDiskSampleData& d_sampleData, 
	int* inNodeIndices, int* inDestStateIndices,
	int* inResults, SerializedStates* inColFreeStates, PoissonDiskSampleData& d_templateData)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, 1, 1);
	dim3 threads = dim3(32, 1, 1);

	CUDA_CHECK_ERROR();

	CUDA_TIMING_BEGIN();

	addNewNodesKernelS <<< grids, threads >>> (inNumQueries, currentTree.numTreeNodes, currentTree.treeNodes, 
		currentTree.treeNodeIndices, currentTree.treeNodeParentIndices, 
		d_sampleData.numSamples, d_sampleData.samples, d_sampleData.levels, d_sampleData.isTreeNode,
		inNodeIndices, inDestStateIndices, inResults, inColFreeStates, d_templateData.samples, *(d_templateData.h_numSamples),
		currentTree.h_RrtTreeId, otherTree.h_RrtTreeId,
		d_solutionTreeId, d_solutionNodeIndex, d_randStates);

	CUDA_TIMING_END("addNewNodesKernel");

	CUDA_CHECK_ERROR();
}

void downloadValues(PoissonDiskSampleData& d_sampleData, RrtTree& currentTree)
{
	CUDA_TIMING_BEGIN();

	FROMGPU(d_sampleData.h_numSamples, d_sampleData.numSamples, sizeof(int));
	FROMGPU(currentTree.h_numTreeNodes, currentTree.numTreeNodes, sizeof(int));

	CUDA_TIMING_END("downloadValues");

}

void generateDestStatesForOtherTree(int inNumQueries, PoissonDiskSampleData& d_sampleData, 
	int* inDestStateIndices, int* inResults, SerializedStates* inColFreeStates, 
	SerializedStates* outStates)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, 1, 1);
	dim3 threads = dim3(7, 32, 1);

	CUDA_TIMING_BEGIN();

	generateDestStatesForOtherTreeKernel <<< grids, threads >>> (inNumQueries, 
		d_sampleData.samples, inDestStateIndices, inResults, inColFreeStates,
		outStates);

	CUDA_TIMING_END("generateDestStatesForOtherTreeKernel");

	CUDA_CHECK_ERROR();
}

bool isSolutionFound(PoissonDiskSampleData& d_sampleData, RrtTree& startTree, RrtTree& goalTree, 
	std::vector<SerializedStates>& solution)
{
	CUDA_TIMING_BEGIN();
	FROMGPU(&h_solutionTreeId, d_solutionTreeId, sizeof(RRT_TREE_ID));
	CUDA_TIMING_END("isSolutionFound");

	bool isFound = h_solutionTreeId != NO_TREE;

	if (isFound)
	{
		// download trees
		int numStartTreeNodes = (*startTree.h_numTreeNodes);
		FROMGPU(h_startTree.treeNodes, startTree.treeNodes, sizeof(SerializedStates) * numStartTreeNodes);
		FROMGPU(h_startTree.treeNodeIndices, startTree.treeNodeIndices, sizeof(int) * numStartTreeNodes);
		FROMGPU(h_startTree.treeNodeParentIndices, startTree.treeNodeParentIndices, sizeof(int) * numStartTreeNodes);

		int numGoalTreeNodes = (*goalTree.h_numTreeNodes);
		FROMGPU(h_goalTree.treeNodes, goalTree.treeNodes, sizeof(SerializedStates) * numGoalTreeNodes);
		FROMGPU(h_goalTree.treeNodeIndices, goalTree.treeNodeIndices, sizeof(int) * numGoalTreeNodes);
		FROMGPU(h_goalTree.treeNodeParentIndices, goalTree.treeNodeParentIndices, sizeof(int) * numGoalTreeNodes);

		int h_solutionNodeIndex;
		FROMGPU(&h_solutionNodeIndex, d_solutionNodeIndex, sizeof(int));

		RrtTree& curTree = (h_solutionTreeId == START_TREE) ? h_startTree : h_goalTree;
		RrtTree& otherTree = (h_solutionTreeId == START_TREE) ? h_goalTree : h_startTree;
		int numOtherTreeNodes = (h_solutionTreeId == START_TREE) ? numGoalTreeNodes : numStartTreeNodes;

		// extract solution
		solution.clear();

		// traverse curTree from h_solutionNodeIndex to root
		int nodeIndex = h_solutionNodeIndex;
		do {
			solution.push_back(curTree.treeNodes[nodeIndex]);
			nodeIndex = curTree.treeNodeParentIndices[nodeIndex];
		} while (nodeIndex != -1);
		// reverse the solution
		std::reverse(solution.begin(), solution.end());

		// find the intersecting node in the other tree
		int intersectingNodeSampleIndex = curTree.treeNodeIndices[h_solutionNodeIndex];
		for (nodeIndex = numOtherTreeNodes - 1; nodeIndex >= 0; --nodeIndex)
		{
			if (otherTree.treeNodeIndices[nodeIndex] == intersectingNodeSampleIndex)
			{
				break;
			}
		}
		// assert (nodeIndex >= 0)

		// skip the intersecting node
		nodeIndex = otherTree.treeNodeParentIndices[nodeIndex];
		// traverse otherTree
		while (nodeIndex != -1)
		{
			solution.push_back(otherTree.treeNodes[nodeIndex]);
			nodeIndex = otherTree.treeNodeParentIndices[nodeIndex];
		}

		// reverse the solution required
		if (h_solutionTreeId == GOAL_TREE)
			std::reverse(solution.begin(), solution.end());
	}

	return isFound;
}

void reset()
{
	h_solutionTreeId = NO_TREE;
	TOGPU(d_solutionTreeId, &h_solutionTreeId, sizeof(RRT_TREE_ID));
}

}

}

////////////////////////////////////////////////////////////////////////////////

__global__ void setDestStatesKernel(int inNumQueries, SerializedStates* inTreeNodeStates, int* inNodeIndices, SerializedStates* inoutDestStates, 
	int* outConnectionCheck, bool isCurrentTree, float maxDistance)
{
	int queryId = blockIdx.x * 32 + threadIdx.x;

	if (queryId < inNumQueries)
	{
		SerializedStates fromState = inTreeNodeStates[inNodeIndices[queryId]];
		SerializedStates toState = inoutDestStates[queryId];
		float distanceR3, distanceSO3;
		computeStateDistance(fromState, toState, distanceR3, distanceSO3);
		float distance = distanceR3 + distanceSO3;

		if (distance > maxDistance)
		{
			// interpolate state
			SerializedStates interpState;

			float t = maxDistance / distance;
			interpolateState(fromState, toState, t, distanceSO3, interpState);

			inoutDestStates[queryId] = interpState;

			if (!isCurrentTree)
				outConnectionCheck[queryId] = 0;
		}

		if (isCurrentTree)
			outConnectionCheck[queryId] = 1;
	}
}

template <class CollideFunctor>
__global__ void checkCollisionKernel2(int inNumQueries, SerializedStates* inTreeNodeStates, int* inTreeNodeIndices, 
	SerializedStates* inDestStates,
	GMesh* robot, GMesh* obstacle, CollideFunctor collideF,
	int* outResult, SerializedStates* outColFreeStates,
	int* inoutConnectionCheck)
{
	__shared__ SerializedStates sharedFromState;
	__shared__ SerializedStates sharedToState;
	__shared__ float sharedDistanceR3;
	__shared__ float sharedDistanceSO3;
	__shared__ SerializedStates sharedInterpolatedStates[COLLISION_THREADS];
	__shared__ int sharedResults[COLLISION_THREADS];

	int queryId = blockIdx.x;
	int sharedDataIdx = threadIdx.x;

	int N = blockDim.x;
	int i = threadIdx.x + 1;

	if (sharedDataIdx == 0)
	{
		sharedFromState = inTreeNodeStates[inTreeNodeIndices[queryId]];
		sharedToState = inDestStates[queryId];
		computeStateDistance(sharedFromState, sharedToState, sharedDistanceR3, sharedDistanceSO3);
	}
	__syncthreads();

	int result = COLLISION_FREE;
	float t = (float)i / N;
	SerializedStates interpolatedState;
	interpolateState(sharedFromState, sharedToState, t, sharedDistanceSO3, interpolatedState);

	/*
	if (componentIdx == 0 && sharedDataIdx == N - 1)
	{
		printf("Interp from (%.3f %.3f %.3f | %.3f %.3f %.3f %.3f) to (%.3f %.3f %.3f | %.3f %.3f %.3f %.3f) at %d :\n(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)\n",
			sharedFromState.x[0],
			sharedFromState.x[1],
			sharedFromState.x[2],
			sharedFromState.x[3],
			sharedFromState.x[4],
			sharedFromState.x[5],
			sharedFromState.x[6],
			sharedToState.x[0],
			sharedToState.x[1],
			sharedToState.x[2],
			sharedToState.x[3],
			sharedToState.x[4],
			sharedToState.x[5],
			sharedToState.x[6],
			sharedDataIdx,
			sharedInterpolatedStates[sharedDataIdx].x[0],
			sharedInterpolatedStates[sharedDataIdx].x[1],
			sharedInterpolatedStates[sharedDataIdx].x[2],
			sharedInterpolatedStates[sharedDataIdx].x[3],
			sharedInterpolatedStates[sharedDataIdx].x[4],
			sharedInterpolatedStates[sharedDataIdx].x[5],
			sharedInterpolatedStates[sharedDataIdx].x[6]);
	}
	*/

	result = collideF(robot->models, robot->vertexPointers, robot->triIdxPointers, &(interpolatedState.x[0]),
		obstacle->models, obstacle->vertexPointers, obstacle->triIdxPointers);

	result = (result == COLLISION_FREE) ? INT_MAX : sharedDataIdx;
	
	sharedResults[sharedDataIdx] = result;
	sharedInterpolatedStates[sharedDataIdx] = interpolatedState;
	__syncthreads();

	// parallel reduction to get min collision index
	if (sharedDataIdx < 32)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 32]);
	}
	if (sharedDataIdx < 16)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 16]);
	}
	if (sharedDataIdx < 8)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 8]);
	}
	if (sharedDataIdx < 4)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 4]);
	}
	if (sharedDataIdx < 2)
	{
		sharedResults[sharedDataIdx] = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 2]);
	}
	if (sharedDataIdx < 1)
	{
		int firstColIdx = min(sharedResults[sharedDataIdx], sharedResults[sharedDataIdx + 1]);

		if (firstColIdx == INT_MAX)
			result = COLLISION_FREE;
		else if (firstColIdx == 0)
			result = IN_COLLISION_WITH_NO_ADVANCE;
		else
		{
			result = IN_COLLISION;
			outColFreeStates[queryId] = sharedInterpolatedStates[firstColIdx - 1];
		}
		outResult[queryId] = result;

		//if (result != COLLISION_FREE)
		if (result == IN_COLLISION_WITH_NO_ADVANCE)
			inoutConnectionCheck[queryId] = 0;

		/*
		printf("Collision Checking from (%.3f %.3f %.3f | %.3f %.3f %.3f %.3f) to (%.3f %.3f %.3f | %.3f %.3f %.3f %.3f) : %d\n",
			sharedFromState.x[0],
			sharedFromState.x[1],
			sharedFromState.x[2],
			sharedFromState.x[3],
			sharedFromState.x[4],
			sharedFromState.x[5],
			sharedFromState.x[6],
			sharedToState.x[0],
			sharedToState.x[1],
			sharedToState.x[2],
			sharedToState.x[3],
			sharedToState.x[4],
			sharedToState.x[5],
			sharedToState.x[6],
			result);
			*/
	}
}

__global__ void addNewNodesKernel2(int inNumQueries, int* numTreeNodes, SerializedStates* treeNodes, 
	int* treeNodeIndices, int* treeNodeParentIndices, 
	int* inNodeIndices, SerializedStates* inDestStates, int* inResults, SerializedStates* inColFreeStates, int currentTreeId)
{
	__shared__ int sharedCollisionResult[32];
	__shared__ int sharedNewNodeId[32];
	__shared__ int sharedSrcTreeNodeId[32];

	int queryId = blockIdx.x * 32 + threadIdx.y;
	int componentIdx = threadIdx.x;
	int sharedDataIdx = threadIdx.y;

	if (queryId < inNumQueries)
	{
		int result = COLLISION_FREE;

		int srcTreeNodeId = -1;

		// copy from global to shared
		if (threadIdx.x == 0)
		{
			result = inResults[queryId];
			sharedCollisionResult[sharedDataIdx] = result;

			srcTreeNodeId = inNodeIndices[queryId];
			sharedSrcTreeNodeId[sharedDataIdx] = srcTreeNodeId;

			if (result != IN_COLLISION_WITH_NO_ADVANCE)
			{
				sharedNewNodeId[sharedDataIdx] = atomicAdd(numTreeNodes, 1);
			}
		}
		__syncthreads();

		result = sharedCollisionResult[sharedDataIdx];
		srcTreeNodeId = sharedSrcTreeNodeId[sharedDataIdx];

		float x;
		if (result == COLLISION_FREE)
		{
			x = inDestStates[sharedDataIdx].x[componentIdx];
		}
		if (result == IN_COLLISION)
		{
			x = inColFreeStates[sharedDataIdx].x[componentIdx];
		}

		// add new node to the tree
		int newNodeId = sharedNewNodeId[sharedDataIdx];
		if (result != IN_COLLISION_WITH_NO_ADVANCE && newNodeId != -1)
		{
			if (threadIdx.x == 0)
			{
				treeNodeParentIndices[newNodeId] = srcTreeNodeId;
			}
			treeNodes[newNodeId].x[componentIdx] = x;
		}

#ifdef DEBUG_NEW_NODE	
		__syncthreads();
		if (result != IN_COLLISION_WITH_NO_ADVANCE && threadIdx.x == 0)
		{
			printf("Tree-%d %d-%d : Node(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)\n", currentTreeId, srcTreeNodeId, newNodeId,
				treeNodes[newNodeId].x[0],
				treeNodes[newNodeId].x[1],
				treeNodes[newNodeId].x[2],
				treeNodes[newNodeId].x[3],
				treeNodes[newNodeId].x[4],
				treeNodes[newNodeId].x[5],
				treeNodes[newNodeId].x[6]);
		}
#endif

	}
}

__global__ void updateDestStatesKernel(int inNumQueries, SerializedStates* inoutDestStates, int* inResults, SerializedStates* inColFreeStates)
{
	__shared__ bool sharedHasCollision[32];

	int queryId = blockIdx.x * 32 + threadIdx.y;
	int componentIdx = threadIdx.x;
	int sharedDataIdx = threadIdx.y;

	if (componentIdx == 0)
	{
		sharedHasCollision[sharedDataIdx] = inResults[queryId];
	}
	__syncthreads();
	if (sharedHasCollision[sharedDataIdx])
	{
		inoutDestStates[queryId].x[componentIdx] = inColFreeStates[queryId].x[componentIdx];
	}

}

namespace rrtconnect
{
namespace cuda
{

void setStartAndGoalStates(SerializedStates* h_startAndGoalStates, PoissonDiskSampleData& d_sampleData,
	RrtTree& d_startTree, RrtTree& d_goalTree)
{
	TOGPU(d_sampleData.samples + 0, h_startAndGoalStates, sizeof(SerializedStates) * 2);
	GPUTOGPU(d_startState, d_sampleData.samples + 0, sizeof(SerializedStates));
	GPUTOGPU(d_goalState, d_sampleData.samples + (0 + 1), sizeof(SerializedStates));

	const RRT_TREE_ID isTreeNode[2] = {START_TREE, GOAL_TREE};
	TOGPU(d_sampleData.isTreeNode + 0, isTreeNode, sizeof(RRT_TREE_ID) * 2);

	int numSamples = 0;
	TOGPU(d_startTree.treeNodeIndices, &numSamples, sizeof(int));
	++numSamples;
	TOGPU(d_goalTree.treeNodeIndices, &numSamples, sizeof(int));
	++numSamples;

	const int noParent = -1;
	TOGPU(d_startTree.treeNodeParentIndices, &noParent, sizeof(int));
	TOGPU(d_goalTree.treeNodeParentIndices, &noParent, sizeof(int));

	unsigned int numStartTreeNodes = 1;
	unsigned int numGoalTreeNodes = 1;

	TOGPU(d_sampleData.numSamples, &numSamples, sizeof(unsigned int));
	*(d_sampleData.h_numSamples) = numSamples;
	TOGPU(d_startTree.numTreeNodes, &numStartTreeNodes, sizeof(int));
	TOGPU(d_goalTree.numTreeNodes, &numGoalTreeNodes, sizeof(int));
	*(d_startTree.h_numTreeNodes) = numStartTreeNodes;
	*(d_goalTree.h_numTreeNodes) = numGoalTreeNodes;

	CUDA_CHECK_ERROR();

#ifdef DEBUG_NEW_NODE	
	printf("Start Node(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)\n",
		h_startAndGoalStates[0].x[0],
		h_startAndGoalStates[0].x[1],
		h_startAndGoalStates[0].x[2],
		h_startAndGoalStates[0].x[3],
		h_startAndGoalStates[0].x[4],
		h_startAndGoalStates[0].x[5],
		h_startAndGoalStates[0].x[6]);
	printf("Goal Node(%.3f %.3f %.3f | %.3f %.3f %.3f %.3f)\n",
		h_startAndGoalStates[1].x[0],
		h_startAndGoalStates[1].x[1],
		h_startAndGoalStates[1].x[2],
		h_startAndGoalStates[1].x[3],
		h_startAndGoalStates[1].x[4],
		h_startAndGoalStates[1].x[5],
		h_startAndGoalStates[1].x[6]);
#endif
}

void uploadData(const std::vector<float>& vBoundsMin, const std::vector<float>& vBoundsMax, 
	const GMesh* h_robot, const GMesh* h_obstacle, 
	GMesh* d_robot, GMesh* d_obstacle, PoissonDiskSampleData& d_sampleData)
{
	CUDA_CHECK_ERROR();

	// precomputed PD samples
	GPUMEMSET(d_sampleData.levels, 0, sizeof(int) * MAX_SAMPLES);
	GPUMEMSET(d_sampleData.isTreeNode, 0, sizeof(RRT_TREE_ID) * MAX_SAMPLES);

	// bounds
	TOGPU_CONSTANT(c_boundsMin, &(vBoundsMin[0]), sizeof(float) * 3);
	TOGPU_CONSTANT(c_boundsMax, &(vBoundsMax[0]), sizeof(float) * 3);
	std::vector<float> vBoundsDiff(3);
	for (int i = 0; i < 3; ++i)
		vBoundsDiff[i] = vBoundsMax[i] - vBoundsMin[i];
	TOGPU_CONSTANT(c_boundsDiff, &(vBoundsDiff[0]), sizeof(float) * 3);

	// meshes
	TOGPU(d_robot, h_robot, sizeof(GMesh));
	TOGPU(d_obstacle, h_obstacle, sizeof(GMesh));

	// termination check
	TOGPU(d_solutionTreeId, &h_solutionTreeId, sizeof(RRT_TREE_ID));
	int h_solutionNodeIndex = -1;
	TOGPU(d_solutionNodeIndex, &h_solutionNodeIndex, sizeof(int));

	CUDA_CHECK_ERROR();
}

void freeMemory(PoissonDiskSampleData& d_sampleData, 
	GMesh* d_robot, GMesh* d_obstacle, RrtTree& d_startTree, RrtTree& d_goalTree)
{
	// precomputed PD samples
	GPUFREE(d_sampleData.numSamples);
	//cudaHostUnregister(d_sampleData.h_numSamples);
	CPUFREE(d_sampleData.h_numSamples);
	GPUFREE(d_sampleData.samples);
	GPUFREE(d_sampleData.levels);
	GPUFREE(d_sampleData.isTreeNode);

	// meshes
	GPUMALLOC(&d_robot, sizeof(GMesh));
	GPUMALLOC(&d_obstacle, sizeof(GMesh));

	// random states
	GPUFREE(d_randStates);

	// start tree
	GPUFREE(d_startTree.numTreeNodes);
	//cudaHostUnregister(d_startTree.h_numTreeNodes);
	CPUFREE(d_startTree.h_numTreeNodes);
	GPUFREE(d_startTree.treeNodeIndices);
	GPUFREE(d_startTree.treeNodeParentIndices);
	GPUFREE(d_startTree.treeNodes);

	// goal tree
	GPUFREE(d_goalTree.numTreeNodes);
	//cudaHostUnregister(d_goalTree.h_numTreeNodes);
	CPUFREE(d_goalTree.h_numTreeNodes);
	GPUFREE(d_goalTree.treeNodeIndices);
	GPUFREE(d_goalTree.treeNodeParentIndices);
	GPUFREE(d_goalTree.treeNodes);

	// termination check
	GPUFREE(d_solutionTreeId);
	GPUFREE(d_solutionNodeIndex);
	//cudaHostUnregister(&h_solutionTreeId);

	// solution retrieve
	CPUFREE(h_startTree.treeNodeIndices);
	CPUFREE(h_startTree.treeNodeParentIndices);
	CPUFREE(h_startTree.treeNodes);
	CPUFREE(h_goalTree.treeNodeIndices);
	CPUFREE(h_goalTree.treeNodeParentIndices);
	CPUFREE(h_goalTree.treeNodes);
}

void setDestStates(int inNumQueries, SerializedStates* inTreeNodeStates, int* inNodeIndices, SerializedStates* inoutDestStates, 
	int* outConnectionCheck, bool isCurrentTree, float maxDistance)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, 1, 1);
	dim3 threads = dim3(32, 1, 1);

	CUDA_TIMING_BEGIN();

	setDestStatesKernel <<< grids, threads >>> (inNumQueries, 
		inTreeNodeStates, inNodeIndices, inoutDestStates, outConnectionCheck, isCurrentTree, maxDistance);

	CUDA_TIMING_END("setDestStatesKernel");

	CUDA_CHECK_ERROR();
}

void checkCollision(int inNumQueries, SerializedStates* inTreeNodeStates, int* inTreeNodeIndices, 
	SerializedStates* inDestStates,
	int* outResult, SerializedStates* outColFreeStates,
	GMesh* d_robot, GMesh* d_obstacle,
	float stateValidityCheckingResolutionR3, float stateValidityCheckingResolutionSO3, int* inoutConnectionCheck)
{
	dim3 grids = dim3(inNumQueries, 1, 1);
	dim3 threads = dim3(COLLISION_THREADS, 1, 1);

	CUDA_TIMING_BEGIN();

	// compute N (# of checking points)
	BVHStateCollideFunctor<OBBNode, OBB, ushort2, IN_COLLISION, unsigned short> collideF;
	checkCollisionKernel2<BVHStateCollideFunctor<OBBNode, OBB, ushort2, IN_COLLISION, unsigned short> > <<< grids, threads >>>(
		inNumQueries, inTreeNodeStates, inTreeNodeIndices, inDestStates,
		d_robot, d_obstacle, collideF,
		outResult, outColFreeStates, inoutConnectionCheck);

	CUDA_TIMING_END("checkCollisionKernel");

	CUDA_CHECK_ERROR();
}

void addNewNodes(int inNumQueries, RrtTree& currentTree, RrtTree& otherTree, 
	int* inNodeIndices, SerializedStates* inDestStates,
	int* inResults, SerializedStates* inColFreeStates)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, 1, 1);
	dim3 threads = dim3(7, 32, 1);

	CUDA_TIMING_BEGIN();

	addNewNodesKernel2 <<< grids, threads >>> (inNumQueries, currentTree.numTreeNodes, currentTree.treeNodes, 
		currentTree.treeNodeIndices, currentTree.treeNodeParentIndices, 
		inNodeIndices, inDestStates, inResults, inColFreeStates, currentTree.h_RrtTreeId);

	CUDA_TIMING_END("addNewNodesKernel");

	CUDA_CHECK_ERROR();
}

void downloadValues(RrtTree& currentTree)
{
	CUDA_TIMING_BEGIN();

	FROMGPU(currentTree.h_numTreeNodes, currentTree.numTreeNodes, sizeof(int));

	CUDA_TIMING_END("downloadValues");
}

void updateDestStates(int inNumQueries, SerializedStates* inoutDestStates, int* inResults, SerializedStates* inColFreeStates)
{
	int gridX = (inNumQueries + 31) / 32;
	dim3 grids = dim3(gridX, 1, 1);
	dim3 threads = dim3(7, 32, 1);

	CUDA_TIMING_BEGIN();

	updateDestStatesKernel <<< grids, threads >>> (inNumQueries, inoutDestStates, inResults, inColFreeStates);

	CUDA_TIMING_END("updateDestStatesKernel");

	CUDA_CHECK_ERROR();
}

bool isSolutionFound(int inNumQueries, RrtTree& currentTree, RrtTree& otherTree, std::vector<SerializedStates>& solution,
	int* inConnectionCheck, int* inCurrentTreeNodeIndices, int* inOtherTreeNodeIndices, SerializedStates* inDestStates)
{
	int solutionIndex = 0;
	CUDA_TIMING_BEGIN();
	std::vector<int> h_connectionCheck(inNumQueries);
	FROMGPU(&h_connectionCheck[0], inConnectionCheck, sizeof(int) * inNumQueries);
	for (solutionIndex = 0; solutionIndex < inNumQueries; ++solutionIndex)
	{
		if (h_connectionCheck[solutionIndex] == 1)
		{
			break;
		}
	}
	CUDA_TIMING_END("isSolutionFound");

	bool isFound = solutionIndex != inNumQueries;

	if (isFound)
	{
		// download trees
		FROMGPU(currentTree.h_numTreeNodes, currentTree.numTreeNodes, sizeof(int));
		FROMGPU(otherTree.h_numTreeNodes, otherTree.numTreeNodes, sizeof(int));

		RrtTree& h_curTree = h_startTree;
		RrtTree& h_otherTree = h_goalTree;
		
		int numCurrentTreeNodes = (*currentTree.h_numTreeNodes);
		FROMGPU(h_curTree.treeNodes, currentTree.treeNodes, sizeof(SerializedStates) * numCurrentTreeNodes);
		FROMGPU(h_curTree.treeNodeParentIndices, currentTree.treeNodeParentIndices, sizeof(int) * numCurrentTreeNodes);

		int numOtherTreeNodes = (*otherTree.h_numTreeNodes);
		FROMGPU(h_otherTree.treeNodes, otherTree.treeNodes, sizeof(SerializedStates) * numOtherTreeNodes);
		FROMGPU(h_otherTree.treeNodeParentIndices, otherTree.treeNodeParentIndices, sizeof(int) * numOtherTreeNodes);

		std::vector<int> h_currentNodeIndices(inNumQueries);
		std::vector<int> h_otherNodeIndices(inNumQueries);
		FROMGPU(&h_currentNodeIndices[0], inCurrentTreeNodeIndices, sizeof(int) * inNumQueries);
		FROMGPU(&h_otherNodeIndices[0], inOtherTreeNodeIndices, sizeof(int) * inNumQueries);

		// extract solution
		solution.clear();

		// traverse curTree from h_solutionNodeIndex to root
		int nodeIndex = h_currentNodeIndices[solutionIndex];
		do {
			solution.push_back(h_curTree.treeNodes[nodeIndex]);
			nodeIndex = h_curTree.treeNodeParentIndices[nodeIndex];
		} while (nodeIndex != -1);
		// reverse the solution
		std::reverse(solution.begin(), solution.end());

		// the last added node
		SerializedStates newState;
		FROMGPU(&(newState.x[0]), inDestStates + solutionIndex, sizeof(SerializedStates));
		solution.push_back(newState);

		// traverse otherTree from h_solutionNodeIndex to root
		nodeIndex = h_otherNodeIndices[solutionIndex];
		do {
			solution.push_back(h_otherTree.treeNodes[nodeIndex]);
			nodeIndex = h_otherTree.treeNodeParentIndices[nodeIndex];
		} while (nodeIndex != -1);
	}

	return isFound;

}

}
}

